//
// Created by ze on 2023/11/2.
//

#include "UPRE_LOCATION.h"
#include "UPRE_GYRO.h"
#include "tim.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_Decision.h"
#include "Kalmanfilter.h"
//全场位姿
Pose Robot_Pose = {0};
//全场定位结构体
Location location = {0};
//是否正在上坡或下坡
bool Slop_Going = false;
//上下坡次数  偶数是将要下坡  奇数是将要上坡
uint8_t Slop_Num = 0;
//坡的长度
#define Slop_len 1295.f
//坡的高度
#define Slop_High 100.f
//上坡走过的距离
float Slop_Cal_len = 0.f;
//记录坡前的坐标
float Slop_len_Compare = 0.f;
//上坡百分比
float Slop_Percent = 0.f;
static uint8_t Go_Num = 0;
//是否可以初始化比较值Slop_len_Compare
static bool Flag_Init = true;
//再次检测上坡
static bool Flag_Again = false;
void Encoder_On()
{
    //码盘读数
    static int32_t Enc1,Enc2,Last_Enc1,Last_Enc2;
    Enc1 = __HAL_TIM_GET_COUNTER(&htim1);
    Enc2 = __HAL_TIM_GET_COUNTER(&htim4);
    //码盘脉冲数微分值

    if(fabsf((float)Enc1 - (float)Last_Enc1) >= 50000.f)
        location.DIS_1 = 0;
    else
        location.DIS_1 =   (float) (Enc1 - Last_Enc1);  //X轴
    if(fabsf((float)Enc2 - (float)Last_Enc2) >= 50000.f)
        location.DIS_2 = 0;
    else
        location.DIS_2 =   (float)-(Enc2 - Last_Enc2);  //Y轴
//    printf("Dis1:%f\tDis_2:%f\r\n",location.DIS_1,location.DIS_2);
#if Printf_Pose  == 1
    printf("X:%f\tY:%f\tPITCH:%f\tROLL:%f\tYAW:%f\r\n",Robot_Pose.wx,Robot_Pose.wy,Robot_Pose.pitch,Robot_Pose.roll,Robot_Pose.yaw);
//printf("PITCH:%f\tIs_Go:%d\tNum:%d\tlen:%f\tPer:%f\tH:%f\tB:%f\r\n",Robot_Pose.pitch,Slop_Going,Slop_Num,Slop_Cal_len,Slop_Percent,High_To_Goal_Top,High_To_Goal_Bottom);
#endif
//    printf("%f\r\n",Robot_Pose.pitch);
//    Flag_ReachZone3 = true;
    /******************************处理谷仓坐标*********************************/
    if(Robot_Pose.wx >= Goal3_x)        //判断东半场还是西半场
    {
        Percept_RePack.DivideBall.direction = right_vent;
        Chassis_Sense_Bool.Robot_In_East_Half = true;
    }
    else
    {
        Percept_RePack.DivideBall.direction = left_vent;
        Chassis_Sense_Bool.Robot_In_East_Half = false;
    }

    if(Chassis_Sense_Bool.Flag_ReachZone3 && fabsf(Robot_Pose.pitch)>=2.f)
    {
        if(Flag_Init == true)
        {
            Go_Num = 1;
            Flag_Init = false;
        }
        Flag_Again = false;
    }
    else if(Robot_Pose.pitch <= 0.8f)
    {
        Flag_Again = true;
    }
    switch (Go_Num) {
        case 1:
            Slop_len_Compare = Robot_Pose.wx;
            Go_Num++;
            break;
        case 2:
            Slop_Going = true;
            Slop_Cal_len = Robot_Pose.wx - Slop_len_Compare;
            Slop_Percent = Slop_Cal_len/Slop_len;
            if(Slop_Num%2==0){
                Chassis_Sense_Value.High_To_Goal_Top = High_Goal + Slop_Percent * Slop_High;
                Chassis_Sense_Value.High_To_Goal_Bottom = Bottom_Goal  + Slop_Percent * Slop_High;
            }
            else if(Slop_Num%2!=0){
                Chassis_Sense_Value.High_To_Goal_Top = High_Goal + Slop_High + Slop_Percent * Slop_High;
                Chassis_Sense_Value.High_To_Goal_Bottom = Bottom_Goal + Slop_High + Slop_Percent * Slop_High;
            }
            if(fabsf(Slop_Percent) >= 0.99f)
                Go_Num++;
            break;
        case 3:
            Slop_Num ++;
            Go_Num++;
            break;
        case 4:
            Slop_Cal_len = 0.f;
            Slop_Percent = 0.f;
            Slop_Going = false;
            if(Slop_Num%2==0)
            {
                Chassis_Sense_Value.High_To_Goal_Bottom = Bottom_Goal;
                Chassis_Sense_Value.High_To_Goal_Top = High_Goal;
            }
            else if(Slop_Num%2!=0)
            {
                Chassis_Sense_Value.High_To_Goal_Top = High_Goal + Slop_High;
                Chassis_Sense_Value.High_To_Goal_Bottom = Bottom_Goal + Slop_High;
            }

            if(Flag_Again == true)
                Flag_Init = true;
            break;
        default:
            break;
    }


    /******************微分***********************/
    Last_Enc1 = Enc1;
    Last_Enc2 = Enc2;
}
void Location_Init(void)
{
    /*************************不要动************************/
//    location.theta = Angle2Radian(45.f - 1.068663917f -0.355318109f-0.317003402f);/*171/9167*/
//    location.theta = Angle2Radian(45.f - 1.155153883f -  0.634549086f + 0.774391697f);
    location.theta = Angle2Radian(45.f);
    /*****************************************************/
    location.gyro_radian = Angle2Radian(gyro.Angle_azimuth);
    location.theta1 = Angle2Radian(Tt1)  + location.gyro_radian;
    location.theta2 = Angle2Radian(Tt2)  + location.gyro_radian;
    location.Sin_12 = sinf(Angle2Radian(Tt1 - Tt2));
    location.Sin_21 = sinf(Angle2Radian(Tt2 - Tt1));
    //距离标定法
//    location.R1 = 100.5f;
//    location.R2 = 100.5f;
//    location.fail1 = Angle2Radian(150.f);
//    location.fail2 = Angle2Radian(36.f);
    //旋转中心标定
//    location.Center_X = 0.f;
//    location.Center_Y = 0.f;
    location.Center_X = 202.901047f;
    location.Center_Y = 58.7293435f;
//    location.RECTANG_X = -0.0132264529f-0.0028152021f;
//    location.RECTANG_X = 0.f;
//    location.RECTANG_Y = 0.f;
//    location.k1 =   arm_cos_f32(location.theta) * PULSE2DIS_ENCODE1  - arm_sin_f32(location.theta) * PULSE2DIS_ENCODE2  * location.RECTANG_X;
//    location.k2 =   arm_sin_f32(location.theta) * PULSE2DIS_ENCODE1  + arm_cos_f32(location.theta) * PULSE2DIS_ENCODE2  * location.RECTANG_X;
//    location.k3 = - arm_sin_f32(location.theta) * PULSE2DIS_ENCODE2  + arm_cos_f32(location.theta) * PULSE2DIS_ENCODE1  * location.RECTANG_Y;
//    location.k4 =   arm_cos_f32(location.theta) * PULSE2DIS_ENCODE2  + arm_sin_f32(location.theta) * PULSE2DIS_ENCODE1  * location.RECTANG_Y;
//    location.g_theta1 = atan2f(-location.k2,location.k4);
//    location.g_theta2 = atan2f(-location.k1,location.k3);

//location.g_pulse_dis1 = location.k1 / (PULSE2DIS_ENCODE1 * arm_sin_f32(location.g_theta2) / location.Sin_21);
//location.g_pulse_dis2 = location.k2 / (PULSE2DIS_ENCODE2 * (-arm_sin_f32(location.g_theta1)) / location.Sin_21);

}
//全场标定函数
void cal_location(void)
{
    static float Last_Pitch = 0.f;
    //逆时针旋转
    Robot_Pose.yaw = gyro.Angle_azimuth;
    if(gyro.Angle_pitch <= 10.f && (gyro.Angle_pitch - Last_Pitch <= 1.f))
        Robot_Pose.pitch = gyro.Angle_pitch;
    Robot_Pose.roll = gyro.Angle_roll;
    location.gyro_radian = Angle2Radian(Robot_Pose.yaw);
    location.ALPHA = location.gyro_radian -location.alphaed;
    location.theta1 = Angle2Radian(Tt1)  + location.gyro_radian;
    location.theta2 = Angle2Radian(Tt2)  + location.gyro_radian;
    /**************************旋转矩阵**********************************************************/
//逆时针旋转矩阵
    location.buf_x = ( (float)location.DIS_1 * cosf(location.theta) + (float)location.DIS_2 * sinf(location.theta)) * PULSE2DIS_ENCODE1;
    location.buf_y = (-(float)location.DIS_1 * sinf(location.theta) + (float)location.DIS_2 * cosf(location.theta)) * PULSE2DIS_ENCODE2;
//    Robot_Pose.wx += (float)location.buf_x;
//    Robot_Pose.wy += (float)location.buf_y;
//    location.spi_Posx = location.buf_x + location.buf_y * location.RECTANG_X;
//    location.spi_Posy = location.buf_y + location.buf_x * location.RECTANG_Y;

    //相对标定法：相对 and 全场直线
//    location.spi_Posx = location.k1 * location.DIS_1 + location.k2 * location.DIS_2;
//    location.spi_Posy = location.k3 * location.DIS_1 + location.k4 * location.DIS_2;

//全场
//location.spi_Posx = (-(float)location.buf_x * arm_sin_f32(location.theta2) + (float)location.buf_y * arm_sin_f32(location.theta1)) / location.Sin_12;
//location.spi_Posy = (-(float)location.buf_x * arm_cos_f32(location.theta2) + (float)location.buf_y * arm_cos_f32(location.theta1)) / location.Sin_21;

    //全场反解公式
//location.spi_Posx = ( DIS2 * sint1 - DIS1 * sint2 + _L1 * arm_cos_f32(location.fail1) *
//        sint2 - _L2 * arm_cos_f32(location.fail2) * sint1 ) / location.Sin_12;
//location.spi_Posy = ( DIS2 * cost1 - DIS1 * cost2 + _L1 *
//        arm_cos_f32(location.fail1) * cost2 - _L2 * arm_cos_f32(location.fail2) *
//        cost1 ) / location.Sin_21;
////全场微分
    location.m_spi_Posx = ( DIS2  * sint1 - DIS1   * sint2  ) / location.Sin_12;
    location.m_spi_Posy = ( DIS2  * cost1 - DIS1   * cost2  ) / location.Sin_21;
//    printf("%f %f %f\r\n",location.m_spi_Posx,location.m_spi_Posy,Robot_Pose.yaw);
////积分部分
    location.spi_Posx += location.m_spi_Posx ;
    location.spi_Posy += location.m_spi_Posy ;
////坐标平移变换
    location.Whole_X = location.spi_Posx - location.Center_X + location.Center_X *
                                                               cosf(location.gyro_radian) - location.Center_Y *
                                                                                            sinf(location.gyro_radian);
    location.Whole_Y = location.spi_Posy - location.Center_Y + location.Center_Y *
                                                               cosf(location.gyro_radian) + location.Center_X *
                                                                                            sinf(location.gyro_radian);

    if(Is_Remake == true && Chassis_Sense_Bool.Off_B_Spline == false)
    {
        Robot_Pose.wx = (float)location.Whole_X + 4979.32f;
        Robot_Pose.wy = (float)location.Whole_Y - 296.56f;
        if(Chassis_Sense_Bool.Start_DT35_Fix && Chassis_Sense_Value.DT35_Fix <= 50.f)
            Robot_Pose.wx = Robot_Pose.wx - Chassis_Sense_Value.DT35_Fix * 1.0373333333333333333333f;

    }
    else
    {
        Robot_Pose.wx = (float)location.Whole_X;
        Robot_Pose.wy = (float)location.Whole_Y;
        if(Chassis_Sense_Bool.Start_DT35_Fix && Chassis_Sense_Value.DT35_Fix <= 50.f)
            Robot_Pose.wx = Robot_Pose.wx - Chassis_Sense_Value.DT35_Fix * 1.0373333333333333333333f;

    }

    /******************微分***********************/
//location.alphaed = location.gyro_radian;
    Last_Pitch = gyro.Angle_pitch;
}